#include <iostream>
#include <vector>
#include <fstream>
#include <string>


#include "cameracalibrator.h"
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>

CameraCalibrator::CameraCalibrator()
{
    flag=0;
    mustInitUndistort = true;
}


CameraCalibrator::~CameraCalibrator()
{
}

int CameraCalibrator::addChessboardPoints(cv::Size &board_size, cv::Mat imageInput)
{
    std::vector< cv::Point2f> imageCorners;
    std::vector<std::vector<cv::Point2f >> image_points_seq;
    std::vector< cv::Point3f> objectCorners;


    if (0 == cv::findChessboardCorners(imageInput,board_size,imageCorners))
    {
        std::cout << "can not find chessboard corners!\n" << std::endl;
        exit(1);
    }
    else
    {
        cv::Mat view_gray;
        cv::cvtColor(imageInput,view_gray,CV_RGB2GRAY);

        cv::find4QuadCornerSubpix(view_gray,imageCorners,cv::Size(11,11));
        image_points_seq.push_back(imageCorners);

        cv::drawChessboardCorners(view_gray,board_size,imageCorners,true);
        cv::imshow("Camera Calibration",view_gray);
        cv::waitKey(500);
    }
    imagePoints.push_back(imageCorners);
    objectPoints.push_back(objectCorners);
}

double CameraCalibrator::calibrate(cv::Size &image_size)
{
    mustInitUndistort = true;
    std::vector<cv::Mat> rvecs, tvecs;
    return  calibrateCamera(objectPoints,imagePoints,image_size,cameraMatrix,distCoeffs,rvecs,tvecs,flag);
}

cv::Mat CameraCalibrator::remap(cv::Mat &imageInput, cv::Size &image_size)
{
    std::cout << "aaa" << std::endl;
    cv::Mat undistorted;

    if (mustInitUndistort)
    {
        std::cout << "mmm" << std::endl;
        cv::initUndistortRectifyMap(cameraMatrix, distCoeffs, R, cameraMatrix, image_size, CV_32FC1, mapx, mapy);
        std::cout << "nnn" << std::endl;
        mustInitUndistort = false;
        std::cout << "vvv" << std::endl;
    }
    std::cout << "bbb" << std::endl;
    cv::remap(imageInput, undistorted, mapx, mapy,cv::INTER_LINEAR);
    std::cout << "ccc" << std::endl;
    return undistorted;
}
